# 本作品采用MIT许可证授权。
# 版权所有 (c) 2013-2023 OpenMV LLC。
# https://github.com/openmv/openmv/blob/master/LICENSE

import sensor
import time
from pyb import UART

# 初始化图像传感器
sensor.reset()
sensor.set_pixformat(sensor.RGB565)  # RGB 彩图
sensor.set_framesize(sensor.QQVGA)   # 160x120
sensor.skip_frames(10)
sensor.set_auto_whitebal(False)
sensor.set_auto_gain(False)
clock = time.clock()

# ----------- 阈值与 ROI 配置 ------------
red_threshold = (17, 100, -2, 127, 6, 127)  # 正确的红色激光 LAB 阈值
roi = (40, 30, 80, 60)  # 中央区域 (x, y, w, h)
# ---------------------------------------

# 串口初始化
uart = UART(3, 115200, timeout_char=200)

# 缓存区用于平滑激光点坐标（滤波效果）
buffer_size = 10
x_buffer = []
y_buffer = []

def append_buffer(buf, value):
    buf.append(value)
    if len(buf) > buffer_size:
        buf.pop(0)

# 查找最大面积的红色目标
def find_max(blobs):
    max_size = 0
    max_blob = None
    for blob in blobs:
        size = blob[2] * blob[3]
        if size > max_size:
            max_blob = blob
            max_size = size
    return max_blob

# 串口发送格式
def sending_data(x, y):
    global uart
    uart.write("$rx: %d, ry: %d#\n" % (x, y))

# 主循环
while True:
    clock.tick()
    img = sensor.snapshot()

    # 可选：画出 ROI 区域
    img.draw_rectangle(roi, color=(255, 255, 0))

    # 识别矩形（如果需要）
    for r in img.find_rects(threshold=10000):
        for p in r.corners():
            img.draw_circle(p[0], p[1], 5, color=(0, 255, 0))  # 绿色角点

    # 在 ROI 中查找红点
    blobs = img.find_blobs([red_threshold], roi=roi)

    if blobs:
        max_blob = find_max(blobs)
        if max_blob:
            cx = max_blob.cx()
            cy = max_blob.cy()

            # 平滑缓存更新
            append_buffer(x_buffer, cx)
            append_buffer(y_buffer, cy)

            avg_x = int(sum(x_buffer) / len(x_buffer))
            avg_y = int(sum(y_buffer) / len(y_buffer))

            # 画出识别框和中心十字
            img.draw_rectangle(max_blob.rect(), color=(255, 0, 0))
            img.draw_cross(avg_x, avg_y, color=(255, 0, 0))

            # 输出坐标
            print(2, avg_x, avg_y)
            sending_data(avg_x, avg_y)

    time.sleep_ms(10)
